/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    FSM.cpp
  * @author  Mentos Seetoo
  * @brief   Finite State Machine base class.
  * @note    
  * @warning 
  *     - At least C++11 is required.	
  * //TODO FSM strcture should be optimized when more states are added.													 
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure there is no any bug in your
  * code. Update the version Number and write dowm your name and the date.What 
  * most important is that make sure the users will have a clear understanding
  * through your description.
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "FSM/FSM.h"

using namespace Quadruped;

template <typename T>
FSM_Base<T> *FSM_Base<T>::current_state = NULL;
template <typename T>
FSM_Base<T> *FSM_Base<T>::next_state = NULL;
template <typename T>
FSM_OPStatus FSM_Base<T>::op_status = FSM_OPStatus::NORMAL;
template <typename T>
std::vector<FSM_Base<T>*> FSM_Base<T>::state_list;
/* Function define -----------------------------------------------------------*/
template <typename T>
FSM_Base<T> *FSM_Base<T>::GetStateHandle(FSM_StateName target_name)
{
    /*Iterate state list*/
    for (auto state : state_list)
        if (state->_name == target_name)
            return state;

    /*Not find desired state*/
    return NULL;
}

template <typename T>
void FSM_Base<T>::InitFSM(FSM_Base<T>* init_state)
{
    current_state = init_state;

    next_state = init_state;

    SwitchState();

    op_status = FSM_OPStatus::NORMAL;
}

template <typename T>
void FSM_Base<T>::RunFSM()
{   
    /*Safety check*/
    /*Safety check passed*/
    if (op_status != FSM_OPStatus::STOP)
    {
        //Run the control code.
        if (op_status == FSM_OPStatus::NORMAL)
        {
            current_state->RunState();
            if (next_state->_name != current_state->_name)
                op_status = FSM_OPStatus::TRANSITIONING;
        }
        
        //State transition.
        if (op_status == FSM_OPStatus::TRANSITIONING)
        {
            if (SwitchState())
                op_status = FSM_OPStatus::NORMAL;
        }
    }
    else
    {
        /*Errors occurred*/
    }
}

template <typename T>
void FSM_Base<T>::SetNextState(FSM_StateName name)
{
    next_state = GetStateHandle(name);
}

/**
* @brief  Functions to control switching states in FSM. It's designed  
*         to be reentrant, in that case we have more time to transit
*         states.
*         This function is called by Run_FSM in every FSM cycle 
*         only when `op_status` is under `FSM_OPStatus::TRANSITIONING`.
* @return true: transition done.
*         false: need to enter function again.
* @author Mentos Seetoo
*/
template <typename T>
bool FSM_Base<T>::SwitchState()
{
    switch(current_state->Transition(next_state))
    {
        //Transition done.
        case 0:
            current_state = next_state;
            return true;
        //Need reentry.
        case 1:
            return false;
        //Error transition, cancel request.
        default:
            next_state = current_state;
            return true;
    }
}

template class FSM_Base<float>;
template class FSM_Base<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
